#include "commData.h"
#include "protocol.h"
#include "CRC_Generate.h"
#include "protocol_grapmotor.h"
#include "GPIO.h"

FunType_bufferTx   grap_lowTx;
/*
note: this part is reference from modbus
sci communication parameter:
speed:9600
checksum:even
stopbit:1
slave add:0x01
*/
grap_resultInfo	grapmotor_result;
grap_info 		grapmotor_proinfo;



list_prot 	grapmotor_queue_tx;
mesg_prot	grapmotor_info[1];
uint8_t u8_grapmotor_acknum = 0;

void queue_insert_grapmotor(pmesg_prot pmesg)
{
	uint8_t pos, status, len, i;
	plist_prot pqueue;
	pqueue = &grapmotor_queue_tx;
	pos = pqueue->head;
	if(mesg_max==pos)pos = 0;
	status = pmesg->status;
	if(0==status){
		len = pmesg->len;
		pqueue->mesg_queue[pos].status = status;
		pqueue->mesg_queue[pos].ID = pmesg->ID;
		pqueue->mesg_queue[pos].len = len;
		for(i=0; i<len; i++){
			pqueue->mesg_queue[pos].buffer[i] = pmesg->buffer[i];
		}
		pqueue->head++;
		if(mesg_max==pqueue->head)pqueue->head=0;
		if(pqueue->size<10)pqueue->size++;		
	}
}
/*
static uint8_t grapmotor_process_NST(uint8_t len, uint8_t data[], pmesg_prot mesg)
{
	uint8_t i;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = mesg->ID;
	for(i=0; i<len; i++){
		Tmesg.buffer[i] = data[i];
	}
	queue_insert_grapmotor(&Tmesg);
	return 0;
}*/


uint8_t grapmotor_process(pmesg_prot mesg)
{
	//uint8_t count;
	uint8_t offset;
	//uint8_t data_Tx[8];
	//len = mesg->len;
	//buffer index from (2~len+2) is the register result
	offset = 3;
	//pwm
	grapmotor_result.pwm = mesg->buffer[offset++];
	grapmotor_result.pwm = (grapmotor_result.pwm<<8)+mesg->buffer[offset++];
	//current
	grapmotor_result.current = mesg->buffer[offset++];
	grapmotor_result.current = (grapmotor_result.current<<8)+mesg->buffer[offset++];
	//freq
	grapmotor_result.freq = mesg->buffer[offset++];
	grapmotor_result.freq = (grapmotor_result.freq<<8)+mesg->buffer[offset++];
	//position control status
	grapmotor_result.pos_cont_status = mesg->buffer[offset++];
	grapmotor_result.pos_cont_status = (grapmotor_result.pos_cont_status<<8)+mesg->buffer[offset++];
	//position high half
	grapmotor_result.pos_high_half = mesg->buffer[offset++];
	grapmotor_result.pos_high_half = (grapmotor_result.pos_high_half<<8)+mesg->buffer[offset++];
	//position low half
	grapmotor_result.pos_low_half = mesg->buffer[offset++];
	grapmotor_result.pos_low_half = (grapmotor_result.pos_low_half<<8)+mesg->buffer[offset++];
	//reserve time high half
	grapmotor_result.res_time_high_half = mesg->buffer[offset++];
	grapmotor_result.res_time_high_half = (grapmotor_result.res_time_high_half<<8)+mesg->buffer[offset++];
	//reserve time low half
	grapmotor_result.res_time_low_half = mesg->buffer[offset++];
	grapmotor_result.res_time_low_half = (grapmotor_result.res_time_low_half<<8)+mesg->buffer[offset++];
	//in1 volt
	grapmotor_result.in1_volt = mesg->buffer[offset++];
	grapmotor_result.in1_volt = (grapmotor_result.in1_volt<<8)+mesg->buffer[offset++];
	//in2 volt
	grapmotor_result.in2_volt = mesg->buffer[offset++];
	grapmotor_result.in2_volt = (grapmotor_result.in2_volt<<8)+mesg->buffer[offset++];
	//in3 volt
	grapmotor_result.in3_volt = mesg->buffer[offset++];
	grapmotor_result.in3_volt = (grapmotor_result.in3_volt<<8)+mesg->buffer[offset++];
	//div volt
	grapmotor_result.div_volt = mesg->buffer[offset++];
	grapmotor_result.div_volt = (grapmotor_result.div_volt<<8)+mesg->buffer[offset++];
	//sq1 volt
	grapmotor_result.sq1_volt = mesg->buffer[offset++];
	grapmotor_result.sq1_volt = (grapmotor_result.sq1_volt<<8)+mesg->buffer[offset++];
	//sq2 volt
	grapmotor_result.sq2_volt = mesg->buffer[offset++];
	grapmotor_result.sq2_volt = (grapmotor_result.sq2_volt<<8)+mesg->buffer[offset++];
	//in1 input pwm duty
	grapmotor_result.in1_input_pwmduty = mesg->buffer[offset++];
	grapmotor_result.in1_input_pwmduty = (grapmotor_result.in1_input_pwmduty<<8)+mesg->buffer[offset++];
	//in1 input freq
	grapmotor_result.in1_input_freq = mesg->buffer[offset++];
	grapmotor_result.in1_input_freq = (grapmotor_result.in1_input_freq<<8)+mesg->buffer[offset++];
	//in1 input pulse count high half
	grapmotor_result.in1_input_pulsecount_high_half = mesg->buffer[offset++];
	grapmotor_result.in1_input_pulsecount_high_half = (grapmotor_result.in1_input_pulsecount_high_half<<8)+mesg->buffer[offset++];
	//in1 input pulse count low half
	grapmotor_result.in1_input_pulsecount_low_half = mesg->buffer[offset++];
	grapmotor_result.in1_input_pulsecount_low_half = (grapmotor_result.in1_input_pulsecount_low_half<<8)+mesg->buffer[offset++];
	//halt status
	grapmotor_result.halt_status = mesg->buffer[offset++];
	grapmotor_result.halt_status = (grapmotor_result.halt_status<<8)+mesg->buffer[offset++];
	//error status
	grapmotor_result.error_status = mesg->buffer[offset++];
	grapmotor_result.error_status = (grapmotor_result.error_status<<8)+mesg->buffer[offset++];
	//motor speed
	grapmotor_result.motor_speed = mesg->buffer[offset++];
	grapmotor_result.motor_speed = (grapmotor_result.motor_speed<<8)+mesg->buffer[offset++];
	//motor speed ratio 1:10
	grapmotor_result.motor_speed_ratio = mesg->buffer[offset++];
	grapmotor_result.motor_speed_ratio = (grapmotor_result.motor_speed_ratio<<8)+mesg->buffer[offset++];	
	//result process
	u8_grapmotor_status = grapmotor_result.error_status;
	u8_grapmotor_acknum = 0;
	return 0;
}

uint8_t grapmotor_rxFunc(void)
{
	return 0;	
}

static uint8_t grapmotor_frametx(pmesg_prot mesg)
{
	uint8_t i, index;
	uint8_t buffer[128];
	uint16_t crc = 0;
	//uint32_t id;
	index = 0;
	//ID insert
	for(i=0; i<1; i++){
		buffer[index++] = (uint8_t)((mesg->ID)>>(8*i));
	}
	//buffer insert
	for(i=0; i<mesg->len; i++){
		buffer[index++] = mesg->buffer[i];
	}
	//CRC INSERT
	//crc = GetCrC16(pqueue->mesg_queue[pos].buffer, len);
	crc = CRC16_comm(buffer, index);
	buffer[index++] = (uint8_t)(crc>>0);
	buffer[index++] = (uint8_t)(crc>>8);
	//tx frame
	grap_lowTx(buffer, index);
	u8_grapmotor_acknum++;
	return 0;
}
uint8_t grapmotor_txFunc(void)
{
	uint8_t result;
	uint8_t pos;
	plist_prot pqueue;
	//second, rx data pro
	pqueue = &grapmotor_queue_tx;
	//size = pqueue->size;
	if(pqueue->size>0){
	//while(pqueue->size>0){
		pos = pqueue->tail;
		if(pos>=mesg_max)pos=0;
		//result pro
		result = grapmotor_frametx(&(pqueue->mesg_queue[pos]));
		pqueue->tail++;
		if(pqueue->tail>=mesg_max)pqueue->tail=0;		
		pqueue->size--;
	}
	return result;
}

uint8_t grapmotor_period_callback(void)
{
	if(grapmotor_proinfo.timer<4){
		grapmotor_proinfo.timer++;
	}else{
		grapmotor_proinfo.timer = 4;
		grapmotor_proinfo.step = 0;
		grapmotor_proinfo.count = 0;
		grapmotor_proinfo.crc16 = 0;
		grapmotor_proinfo.crc16_T = 0;
		grapmotor_proinfo.len = 0;
	}
	return 0;
}
/*
ask:
01 03 00 20 00 15 85 CF 
resp:
01 03 2A 
00 00 00 00 00 
00 00 01 FF FF 
F8 0D 00 00 00 
00 0C DE 0C DE 
0C E1 00 00 00 
01 00 01 03 E8 
00 00 00 00 00 
00 00 00 00 00 
FF FF 
E7 1C
note: 
    the u8_grapmotor_idealtimer should be update by timer
*/

void grapmotor_rxFunc_callback(uint8_t data_rx)
{
	static uint16_t crc_t = 0;
	grap_Pinfo PproT;	
	PproT = &grapmotor_proinfo;
	//first, start flag check
	if(PproT->timer>3){		
		grapmotor_info[0].status=1;
		grapmotor_info[0].ID = data_rx;
		PproT->count = 0;
		if(grapmotor_ID==data_rx){
			PproT->step = 1;
			return;
		}				
	}else{
		if(0==PproT->step){
			if(grapmotor_ID==data_rx){
				PproT->step = 1;
				PproT->count = 0;
				grapmotor_info[0].status=1;
				grapmotor_info[0].ID = data_rx;
				grapmotor_info[0].buffer[PproT->count++] = data_rx;
				return;
			}
		}
	}
	PproT->timer = 0;
	switch(PproT->step){
		case(1):
			//second, cmd check
			if(0x03==data_rx){
				grapmotor_info[0].buffer[PproT->count++] = data_rx;
				PproT->step = 0x02;
			}else{
				//reset
				PproT->step = 0;
				PproT->count = 0;
				grapmotor_info[0].status=0;
			}
		break;
		case(2):
			//third, len check
			grapmotor_info[0].buffer[PproT->count++] = data_rx;
			PproT->len = grapmotor_info[0].len = data_rx;
			PproT->step = 3;
		break;
		case(3):
			//four, data buffer
			if(PproT->count<(PproT->len+3)){
				grapmotor_info[0].buffer[PproT->count++] = data_rx;
			}else{
				//five, checksum
				PproT->crc16 = CRC16_comm(grapmotor_info[0].buffer, PproT->count);
				grapmotor_info[0].buffer[PproT->count++] = data_rx;
				crc_t = data_rx;
				PproT->step = 4;
			}
		break;
		case(4):
			//six, end check
			PproT->crc16_T = data_rx;
			PproT->crc16_T = (PproT->crc16_T<<8) + crc_t;
			grapmotor_info[0].buffer[PproT->count++] = data_rx;
			if(PproT->crc16 ==PproT->crc16_T)
			{
				grapmotor_info[0].status=0;
				//insert data
				list_insert(&grapmotor_info[0],grapmotor_group);
			}
			//reset
			PproT->step = 0;
			PproT->count = 0;
		break;
		default:
			PproT->step = 0;
			PproT->count = 0;
		break;
	}
}

/*
Cmds
*/

/*
status check:id 03 00 10 00 0B crc_high crc_low
*/
uint8_t grapmotor_status_Get(void)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = grapmotor_ID;
	count = 0;
	Tmesg.buffer[count++] = 0x03;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x10;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x0B;
	Tmesg.len = count;
	queue_insert_grapmotor(&Tmesg);
	u8_grapmotor_acknum++;
	return 0;
}
/*
02 10 
00 80 00 10 20 
00 02 00 00 00 01 00 00 00 00 00 00 00 28 00 28 00 3C 00 00 00 00 25 80 00 02 00 00 00 00 00 00 
C9 E8 
ErrorStopTime->0.1s
NormalCur->0.01A
MaxCur->0.01A
StopCur->0.01A
LostStopTime->0.1s
*/
uint8_t grapmotor_Sysconfigure(uint8_t ErrorStopTime, uint16_t NormalCur, uint16_t MaxCur, uint16_t StopCur, uint8_t LostStopTime)
{
	uint8_t SysConfig[39] = {0x02,
							0x10,0x00,0x80,0x00,
							0x10,0x20,
							0x00,0x02,0x00,0x00,0x00,0x01,0x00,0x00,
							0x00,0x00,0x00,0x00,0x00,0x28,0x00,0x28,
							0x00,0x3C,0x00,0x00,0x00,0x00,0x25,0x80,
							0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00};
	uint8_t count;
	mesg_prot Tmesg;
	SysConfig[1*2] = (uint8_t)(ErrorStopTime>>8);
	SysConfig[1*2+1] = (uint8_t)(ErrorStopTime>>0);
	SysConfig[6*2] = (uint8_t)(NormalCur>>8);
	SysConfig[6*2+1] = (uint8_t)(NormalCur>>0);
	SysConfig[7*2] = (uint8_t)(MaxCur>>8);
	SysConfig[7*2+1] = (uint8_t)(MaxCur>>0);
	SysConfig[8*2] = (uint8_t)(StopCur>>8);
	SysConfig[8*2+1] = (uint8_t)(StopCur>>0);
	SysConfig[14*2] = (uint8_t)(LostStopTime>>8);
	SysConfig[14*2+1] = (uint8_t)(LostStopTime>>0);
	Tmesg.status = 0;
	Tmesg.ID = grapmotor_ID;
	for(count=0; count<39; count++){
		Tmesg.buffer[count] = SysConfig[count];
	}
	Tmesg.len = count;
	queue_insert_grapmotor(&Tmesg);
	return 0;
}
/*
setting speed and position:
    ID 06 00 40 speed_high speed_low crc_high crc_low
note:
  the speedHz is 0.1hz为精度
  the positionLen has signed  
*/
uint8_t grapmotor_speed_Set(int16_t speedHz)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = grapmotor_ID;
	count = 0;
	Tmesg.buffer[count++] = 0x06;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x40;	
	Tmesg.buffer[count++] = (uint8_t)(speedHz>>8);
	Tmesg.buffer[count++] = (uint8_t)(speedHz>>0);
	Tmesg.len = count;
	queue_insert_grapmotor(&Tmesg);
	return 0;
}


/*
user can use this function to change low tx function
*/
void grapmotor_lowTxfunc_Register(FunType_bufferTx txFunc)
{
	grap_lowTx = txFunc;
}

void grapmotor_init(FunType_bufferTx txFunc)
{
	//register function
	func_register_init(grapmotor_group, grapmotor_rxFunc, grapmotor_txFunc, grapmotor_process);
	grap_lowTx = txFunc;
}


uint8_t grapmotor_step_setting(uint8_t dirc, uint16_t sped, uint16_t time)
{
	if(Larger_grap==dirc){
		sped = sped;
	}else{
		sped = 0- sped;
	}
	grapmotor_speed_Set(sped);
	list_check(grapmotor_group);
	wait_for_timeout(time);
	grapmotor_speed_Set(0);
	list_check(grapmotor_group);
	wait_for_timeout(50);
	grapmotor_status_Get();
	list_check(grapmotor_group);
	wait_for_timeout(50);
	return 0;
}

uint8_t grapmotor_sped_setting(uint8_t dirc, uint16_t sped)
{
	if(Larger_grap==dirc){
		sped = sped;
	}else{
		sped = 0- sped;
	}
	grapmotor_speed_Set(sped);
	list_check(grapmotor_group);
	return 0;
}


uint8_t grapmotor_firstPosition_Set(void)
{
	uint8_t result = 0;
	uint8_t firstPos_Volt = 0;
	uint8_t tryCount = 0;
	uint8_t i = 0;
	uint8_t StopCount, ActionStop;
	uint16_t NormCurrentT[2] = {30, 40};
	uint16_t MaxCurrentT[2] = {40, 40};
	uint16_t StopCurrentT[2] = {40, 40};
	uint8_t  LostTime[2] = {5,5};
	uint8_t  ErrTime[2] = {0, 0};

	grapmotor_step_setting(Larger_grap, 0, 100);
	//first, check the first position volt
	firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);	
	for(i=0; i<1; i++)
	{
		grapmotor_Sysconfigure(ErrTime[i], NormCurrentT[i], MaxCurrentT[i], StopCurrentT[i], LostTime[i]);
		list_check(grapmotor_group);
		wait_for_timeout(50);
		grapmotor_sped_setting(Larger_grap, 200);
		list_check(grapmotor_group);
		wait_for_timeout(50);
		tryCount = 0;
		ActionStop = 0;
		while(0==ActionStop){		
			//wait_for_timeout(100);
			grapmotor_status_Get();			
			list_check(grapmotor_group);
			wait_for_timeout(100);
			if(grapmotor_result.current>10){
				StopCount++;
				firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);	
				if(0!=firstPos_Volt){
					if(StopCount>=1){
						ActionStop = 1;
						result = 1;
						break;
					}
				}else{
					if(StopCount>=1){
						ActionStop = 1;
						result = 0;
						break;
					}
				}
			}else{
				tryCount++;
				if(tryCount>1){
					result = 1;
					break;
				}
			}
		}
	}
	//grapmotor_sped_setting(Smaller_grap, 50);
	//list_check(grapmotor_group);
	//wait_for_timeout(50);
	grapmotor_sped_setting(Smaller_grap, 0);
	list_check(grapmotor_group);
	return result;
}

uint8_t Ingrap_Action_grap(void)
{	
	uint8_t result = 0;
	uint8_t tryCount = 0;
	uint8_t ActionStop = 0;
	uint8_t StopCount = 0;

	grapmotor_step_setting(Larger_grap, 0, 100);
	while(1!=ActionStop){
		grapmotor_step_setting(Smaller_grap, 100, 100);
		//current check		
		if(grapmotor_result.current>10){
			StopCount++;
			if(StopCount>=2){
				ActionStop = 1;
				break;
			}
		}else{
			tryCount++;
			if(tryCount>50){
				result = 1;
				break;
			}
		}
		//usart1Printf("grapmotor current:%d~",grapmotor_result.current);
		wait_for_timeout(100);
	}
	return result;
}

uint8_t Ingrap_Action_grap_v2(void)
{	
	uint8_t result = 0;
	uint8_t tryCount = 0;
	uint8_t ActionStop = 0;
	uint8_t StopCount = 0;
	uint8_t i = 0;
	uint16_t NormCurrentT[3] = {30, 250, 120};//{30, 120};
	uint16_t MaxCurrentT[3] = {40, 300, 150};//{40, 150};
	uint16_t StopCurrentT[3] = {40, 300, 150};//{40, 150};
	uint8_t  LostTime[3] = {5,5, 5};
	uint8_t  ErrTime[3] = {0, 0, 0};

	grapmotor_sped_setting(Larger_grap, 0);
	wait_for_timeout(50);
	grapmotor_sped_setting(Smaller_grap, 200);
	wait_for_timeout(50);
	//first position check
	//setting first max current
	//second position check_grappower
	//setting second max current
	for(i=0; i<3; i++)
	{
		//setting current
		grapmotor_Sysconfigure(ErrTime[i], NormCurrentT[i], MaxCurrentT[i], StopCurrentT[i], LostTime[i]);
		list_check(grapmotor_group);
		wait_for_timeout(100);
		ActionStop = 0;
		tryCount = 0;
        StopCount = 0;
		while(0==ActionStop){		
			//wait_for_timeout(100);
			//current check	
			grapmotor_status_Get();			
			list_check(grapmotor_group);
			wait_for_timeout(50);
			if(grapmotor_result.current>NormCurrentT[i]){
				StopCount++;
				if(StopCount>=1){
					ActionStop = 1;
					break;
				}
			}else{
				tryCount++;
				if(tryCount>=1){
					result = 1;
					break;
				}
			}
		}
	}
	return result;
}

/*
this function should be call by application periodly
note: the speed set should be set by user condition
*/
static void grapmotor_funcTest(void)
{
	static uint8_t loop = 0;
	loop++;
	//uint16_t speedHz = 2000;//0.1hz为精度
	//uint16_t positionMode = 1;//1->相对位置；0->绝对位置
	//uint16_t positionLen = 100;
	if(loop%2==1){
		grapmotor_speed_Set(100);
	}else{
		grapmotor_speed_Set(-100);
	}
}
/*
note : this is only a function test interface, not use for real function
*/
uint8_t grapmotor_interfaceTest(void)
{
	uint8_t result = 0;
	//grap motor status get
	grapmotor_status_Get();
	//grapmotor_funcTest();
	//result = list_check(grapmotor_group);
	return result;
}
uint8_t grapmotor_task(void)
{
	uint8_t result = 0;
	//grap motor status get
	grapmotor_status_Get();
	//grapmotor_funcTest();
	result = list_check(grapmotor_group);
	return result;
}


